#include "sdShip.h"

void sdShip::move(const cpVect& direction) {
	cpFloat angle = body_->a;
	
	//TODO: CHANGE ALL SIN() COS() TO A TABLE FOR FASTER SPEED
	cpFloat c = cos(angle);
	cpFloat s = -sin(angle);
	
	cpVect force = cpv(0, 0);
	if(direction.x < 0) {
		force.x += direction.x * leftForce_ * c;
		force.y += -direction.x * leftForce_ * s;
	} else {
		force.x += direction.x * rightForce_ * c;
		force.y += -direction.x * rightForce_ * s;
	}
	
	if(direction.y < 0) {
		force.x += direction.y * reverseForce_ * s;
		force.y += direction.y * reverseForce_ * c;
	} else {
	    force.x += direction.y * forwardsForce_ * s;
		force.y += direction.y * forwardsForce_ * c;
	}
	
	body_->f = force;
}

void sdShip::turn(const cpVect& direction) {
	cpFloat shipToMouseAngle = -cpvtoangle(cpvsub(direction, body_->p));
	cpFloat turn = (shipToMouseAngle - body_->a);
	
    if(turn > 3.14159) {
        turn -= (2 * 3.14159);
    } else if(turn < -3.14159) {
        turn += (2 * 3.14159);
    }
    
    turn *= deltaW_;

	body_->w = turn;
}

bool sdShip::shoot(const int weapon, sdTimed& bullet) {
    if(weapon == 1) {
        //sdObject* bullet = new sdObject(cpBodyNew(0, 0), getSpace());
        //bullets_.push_back(bullet);
        
    	int num = 4;
        cpVect verts[] = {
    		cpv(-0.1, -4),
    		cpv(-0.1,  4),
    		cpv( 0.1,  4),
    		cpv( 0.1, -4),
    	};
    	
    	bullet.setMass(5.0);
    	bullet.setPos(cpv(body_->p.x + (25 * sin(-body_->a)), body_->p.y + (25 * cos(-body_->a))));
    	
        bullet.addPolyShape(num, verts, cpvzero, 2);
    	
    	bullet.setAngle(body_->a);
    	bullet.setVel(cpv(2000 * sin(-body_->a), 2000 * cos(-body_->a)));
    	//universe_.insert(bullet);
    	
    	//bullets_.push_back(bullet);
    	//TODO: change to bullet
    		
    	//body = cpBodyNew(6.0, cpMomentForPoly(6.0, num, verts, cpvzero));
    	//body->p = cpv(sBody->p.x + (25 * sin(-sBody->a)), sBody->p.y + (25 * cos(-sBody->a)));
    	//body->a = sBody->a;
    	//body->v = cpv((2000 * sin(-sBody->a)), (2000 * cos(-sBody->a)));
    	//cpSpaceAddBody(space, body);
    	//shape = cpPolyShapeNew(body, num, verts, cpvzero);
    	
    	//shape->e = 0.4; shape->u = 0.01;
    	//shape->collision_type = 1;
    	//cpSpaceAddShape(space, shape);
    	
    	return true;
    } else if(weapon == 2) {
    
    }
    
    return false;
}

void sdShip::getChunks(std::set<sdTimed*>* chunks) {
    int count = 80;
    cpFloat div = 2 * 3.14159 / count;
    
    for(int i = 0; i < count; i++) {
        cpBody* body = new cpBody();
    
        sdTimed* bullet = new sdTimed(100.0, body);
        bullet->setMass(5.0);
    	bullet->setPos(cpv(body_->p.x + (25 * sin(-body_->a)), body_->p.y + (25 * cos(-body_->a))));
    	bullet->addCircleShape(0, 4, cpvzero, 4);

    	bullet->setVel(cpv(2000 * sin(i * div), 2000 * cos(i * div)));
 	
    	chunks->insert(bullet);
    }
}
